#include "image_data_collect.h"

using namespace std;

/**************************************************************************************
功能描述: 数据采集
修改记录:
**************************************************************************************/

void IMAGE_MSG_RECEIVER_C::MsgReceiveCallBack(const sensor_msgs::Image::ConstPtr& imageMsg)
{
    cv_bridge::CvImagePtr cv_image;
    try {
        cv_image = cv_bridge::toCvCopy(imageMsg, "bgr8");
    } catch(cv_bridge::Exception& e){
        ROS_ERROR("Could not convert for '%s' to 'bgr8'.", imageMsg->encoding.c_str());
        usleep(1 * 1000);
        return;
    }
    image_ = cv_image->image;
}

void IMAGE_MSG_RECEIVER_C::GetNormalImage(cv::Mat& image)
{
    image = image_;
}


int IMAGE_DATA_COLLECT_C::Init()
{
    strPrefix_ = GetPrefix(cameraName_);

    if (strPrefix_.empty()) {
        ST_LOG_ERR("%s Directory Not Exist. Init Fail.", strPrefix_.c_str());
        return -1;
    }
    return 0;
}

void IMAGE_DATA_COLLECT_C::DataCollect()
{
    cv::Mat image;
    objImageMsgRec_.GetNormalImage(image);

    //double tt = image->header.stamp / 1000000.0;
    double tt;
    string fileName = strPrefix_ + std::to_string(tt) + ".jpg";
    cv::imwrite(fileName, image);
}

string IMAGE_DATA_COLLECT_C::GetPrefix(const string &cameraName)
{
    string strPrefix;
    
    if (cameraName == FRONT_UPPER_CAMERA_TOPIC)
        strPrefix = objDataFile_.GetFrontUpperCamDirPath() + "/0_";
    else if (cameraName == FRONT_LOWER_CAMERA_TOPIC)
        strPrefix = objDataFile_.GetFrontLowerCamDirPath() + "/1_";
    else if (cameraName == FRONT_LEFT_CAMERA_TOPIC)
        strPrefix = objDataFile_.GetFrontLeftCamDirPath() + "/2_";
    else if (cameraName == FRONT_RIGHT_CAMERA_TOPIC)
        strPrefix = objDataFile_.GetFrontRightCamDirPath() + "/3_";
    else if (cameraName == REAR_CAMERA_TOPIC)
        strPrefix = objDataFile_.GetRearCamDirPath() + "/4_";
    else if (cameraName == REAR_LEFT_CAMERA_TOPIC)
        strPrefix = objDataFile_.GetRearLeftCamDirPath() + "/5_";
    else if (cameraName == REAR_RIGHT_CAMERA_TOPIC)
        strPrefix = objDataFile_.GetRearRightCamDirPath() + "/6_";
    else if (cameraName == TRAFFIC_LIGHT_CAMERA_TOPIC)
        strPrefix = objDataFile_.GetTrafficLightCamDirPath() + "/7_";
    else
        strPrefix = objDataFile_.GetFrontLeftCamDirPath() + "/null_";

    return strPrefix;
}

